#include "../include/Frame.hpp"
#include "../include/ORBFeature.hpp"
using namespace std;
using namespace cv;
namespace ORBSLAM
{
    long unsigned int Frame::sm_frame_curid = 0;
    long unsigned int KeyFrame::sm_keyframe_curid = 0;
    bool Frame::mb_initial_image_bound = true;
    float Frame::mf_grid_element_width_inv;
    float Frame::mf_grid_element_height_inv;
    float Frame::mf_box_minx;
    float Frame::mf_box_miny;
    float Frame::mf_box_maxx;
    float Frame::mf_box_maxy;
    Frame::Frame() {}

    KeyFrame::KeyFrame(Frame &frame) : Frame(frame), mb_current_place_recognition(false), mb_not_erase(false), mb_to_be_erase(false), mn_place_recognition_query(0), mn_place_recognition_worlds(0), mf_place_recognition_score(0.0), mn_relocal_worlds(0)
    {
        mb_bad = false;
        mn_keyframe_id = sm_keyframe_curid++;
        mb_first_connect = true;
        mp_parent = static_cast<KeyFrame *>(NULL);
    }
    KeyFrame::KeyFrame()
    {
        mb_bad = false;
        mn_keyframe_id = sm_keyframe_curid++;
        mb_first_connect = true;
        mp_parent = static_cast<KeyFrame *>(NULL);
    }
    void KeyFrame::Init_keyframe()
    {
        mn_localkeyframe_tmpid = -1;

        mn_mpts_num = 0;

        for (auto p_mpt : mvp_mappoints)
        {
            if (!p_mpt)
            {
                continue;
            }
            if (p_mpt->Is_bad())
            {
                continue;
            }
            mn_mpts_num++;
        }

        mn_mark_fuse_id = 0;
        mn_mark_localba_id = 0;
        mn_mark_localba_fixed_id = 0;
        mb_not_erase = false;
        mb_used_as_ref_keyframe = false;
        mn_relocal_query = 0;
        mf_relocal_score = 0;
    }

    bool KeyFrame::Is_bad()
    {
        unique_lock<mutex> lock(mmutex_connection);
        return mb_bad;
    }
    Map *KeyFrame::Get_map()
    {
        unique_lock<mutex> lock(mmutex_map);
        return mp_map;
    }

    Frame::Frame(const Mat &im, ORBFeature *orbfeature, int feature_num_to_extract, double time_stamp)
    {
        mn_frame_id = sm_frame_curid++;
        m_time_stamp = time_stamp;
        m_image = im.clone();
        orbfeature->extract_orb_compute_descriptor(im, feature_num_to_extract, mv_orb_keypoints, mm_descriptors);
        mn_feature_num = mv_orb_keypoints.size();
        // cout << "feature num:" << mn_feature_num << endl;
        mvp_mappoints = vector<MapPoint *>(mn_feature_num, static_cast<MapPoint *>(NULL));
        // mn_mpts_num = 0;
        mvb_outline = vector<bool>(mn_feature_num, false);
        orbfeature->undistort_keypoints(mv_orb_keypoints, mv_orb_unkeypoints, mn_feature_num);
        if (mb_initial_image_bound)
        {
            orbfeature->compute_image_bounds(im, mf_box_minx, mf_box_miny, mf_box_maxx, mf_box_maxy);
            mf_grid_element_width_inv = static_cast<float>(FRAME_GRID_COLS) / static_cast<float>(mf_box_maxx - mf_box_minx);
            mf_grid_element_height_inv = static_cast<float>(FRAME_GRID_ROWS) / static_cast<float>(mf_box_maxy - mf_box_miny);
            mb_initial_image_bound = false;
        }
        assign_features_to_grid();
    }

    void Frame::assign_features_to_grid()
    {
        int cols = FRAME_GRID_COLS;
        int rows = FRAME_GRID_ROWS;
        const int ncells = rows * cols;
        int nreserve = 0.5f * mn_feature_num / ncells;
        for (int i = 0; i < rows; i++)
        {
            for (int j = 0; j < cols; j++)
            {
                mv_grid[j][i].reserve(nreserve);
            }
        }
        for (int i = 0; i < mn_feature_num; i++)
        {
            const KeyPoint &kp = mv_orb_unkeypoints[i];
            int row, col;
            if (Pos_in_grid(kp, row, col))
            {
                mv_grid[col][row].push_back(i);
                // cout<<"grid:"<<mv_grid[col][row].size()<<endl;
            }
        }
    }
    bool Frame::Pos_in_grid(const KeyPoint &kp, int &row, int &col)
    {
        col = cvFloor((kp.pt.x - mf_box_minx) * mf_grid_element_width_inv);
        row = cvFloor((kp.pt.y - mf_box_miny) * mf_grid_element_height_inv);
        if (col < 0 || col >= FRAME_GRID_COLS || row < 0 || row >= FRAME_GRID_ROWS)
        {
            mn_feature_num--;
            return false;
        }
        return true;
    }

    vector<int> Frame::Get_candidate_points_to_match(const Point2f &vp_prematched, int window_size, int min_level, int max_level)
    {
        float x = vp_prematched.x;
        float y = vp_prematched.y;
        vector<int> candidate_index;
        candidate_index.reserve(mn_feature_num);
        float factor_x = (float)window_size;
        float factor_y = (float)window_size;
        const int min_xindex = max(0, (int)floor((x - mf_box_minx - factor_x) * mf_grid_element_width_inv));
        if (min_xindex >= FRAME_GRID_COLS)
        {
            return candidate_index;
        }
        const int max_xindex = min((int)FRAME_GRID_COLS - 1, (int)floor((x - mf_box_minx + factor_x) * mf_grid_element_width_inv));
        if (max_xindex < 0)
        {
            return candidate_index;
        }
        const int min_yindex = max(0, (int)floor((y - mf_box_miny - factor_y) * mf_grid_element_height_inv));
        if (min_yindex >= FRAME_GRID_ROWS)
        {
            return candidate_index;
        }
        const int max_yindex = min((int)FRAME_GRID_ROWS - 1, (int)floor((y - mf_box_miny + factor_y) * mf_grid_element_height_inv));
        if (max_yindex < 0)
        {
            return candidate_index;
        }
        for (int ix = min_xindex; ix <= max_xindex; ix++)
        {
            for (int iy = min_yindex; iy <= max_yindex; iy++)
            {
                vector<int> kps_in_cell = mv_grid[ix][iy];
                if (kps_in_cell.empty())
                {
                    continue;
                }
                for (int i = 0; i < kps_in_cell.size(); i++)
                {
                    KeyPoint kp = mv_orb_unkeypoints[kps_in_cell[i]];
                    // TODO different from orign code
                    if (kp.octave < min_level || kp.octave > max_level)
                    {
                        continue;
                    }
                    // const float distx = kp.pt.x - x;
                    // const float disty = kp.pt.y - y;
                    // if (fabs(distx) < factor_x && fabs(disty) < factor_y)
                    // {
                    //     candidate_index.push_back(kps_in_cell[i]);
                    // }
                    candidate_index.push_back(kps_in_cell[i]);
                }
            }
        }
        return candidate_index;
    }

    void Frame::Set_pose(const Mat &Tcw)
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        mm_Tcw = Tcw.clone();
        mm_Rcw = mm_Tcw.rowRange(0, 3).colRange(0, 3);
        mm_tcw = mm_Tcw.rowRange(0, 3).col(3);
        mm_Rwc = mm_Rcw.t();
        mm_twc = -mm_Rwc * mm_tcw;
        mm_Twc = Mat::eye(4, 4, CV_32F);
        mm_Rwc.copyTo(mm_Twc.rowRange(0, 3).colRange(0, 3));
        mm_twc.copyTo(mm_Twc.rowRange(0, 3).col(3));

        this->mm_Tcw_ = Matx44f(mm_Tcw.at<float>(0, 0), mm_Tcw.at<float>(0, 1), mm_Tcw.at<float>(0, 2), mm_Tcw.at<float>(0, 3), mm_Tcw.at<float>(1, 0), mm_Tcw.at<float>(1, 1), mm_Tcw.at<float>(1, 2), mm_Tcw.at<float>(1, 3), mm_Tcw.at<float>(2, 0), mm_Tcw.at<float>(2, 1), mm_Tcw.at<float>(2, 2), mm_Tcw.at<float>(2, 3), mm_Tcw.at<float>(3, 0), mm_Tcw.at<float>(3, 1), mm_Tcw.at<float>(3, 2), mm_Tcw.at<float>(3, 3));
        this->mm_Rcw_ = Matx33f(mm_Rcw.at<float>(0, 0), mm_Rcw.at<float>(0, 1), mm_Rcw.at<float>(0, 2), mm_Rcw.at<float>(1, 0), mm_Rcw.at<float>(1, 1), mm_Rcw.at<float>(1, 2), mm_Rcw.at<float>(2, 0), mm_Rcw.at<float>(2, 1), mm_Rcw.at<float>(2, 2));
        this->mm_tcw_ = Matx31f(mm_tcw.at<float>(0), mm_tcw.at<float>(1), mm_tcw.at<float>(2));
        this->mm_Twc_ = Matx44f(mm_Twc.at<float>(0, 0), mm_Twc.at<float>(0, 1), mm_Twc.at<float>(0, 2), mm_Twc.at<float>(0, 3), mm_Twc.at<float>(1, 0), mm_Twc.at<float>(1, 1), mm_Twc.at<float>(1, 2), mm_Twc.at<float>(1, 3), mm_Twc.at<float>(2, 0), mm_Twc.at<float>(2, 1), mm_Twc.at<float>(2, 2), mm_Twc.at<float>(2, 3), mm_Twc.at<float>(3, 0), mm_Twc.at<float>(3, 1), mm_Twc.at<float>(3, 2), mm_Twc.at<float>(3, 3));
        this->mm_Rwc_ = Matx33f(mm_Rwc.at<float>(0, 0), mm_Rwc.at<float>(0, 1), mm_Rwc.at<float>(0, 2), mm_Rwc.at<float>(1, 0), mm_Rwc.at<float>(1, 1), mm_Rwc.at<float>(1, 2), mm_Rwc.at<float>(2, 0), mm_Rwc.at<float>(2, 1), mm_Rwc.at<float>(2, 2));
        this->mm_twc_ = Matx31f(mm_twc.at<float>(0), mm_twc.at<float>(1), mm_twc.at<float>(2));
    }

    Matx31f Frame::Get_tcw_()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_tcw_;
    }

    Matx31f Frame::Get_twc_()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_twc_;
    }

    Matx33f Frame::Get_Rwc_()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Rwc_;
    }

    Matx33f Frame::Get_Rcw_()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Rcw_;
    }

    Matx44f Frame::Get_Tcw_()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Tcw_;
    }

    Matx44f Frame::Get_Twc_()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Twc_;
    }

    Mat Frame::Get_tcw()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_tcw.clone();
    }

    Mat Frame::Get_twc()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_twc.clone();
    }

    Mat Frame::Get_Rwc()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Rwc.clone();
    }

    Mat Frame::Get_Rcw()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Rcw.clone();
    }

    Mat Frame::Get_Tcw()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Tcw.clone();
    }

    Mat Frame::Get_Twc()
    {
        unique_lock<mutex> lock(*mp_mutex_frame_pose);
        return mm_Twc.clone();
    }

    void KeyFrame::Add_mappoint(MapPoint *p_mappoint, const int &idx)
    {
        unique_lock<mutex> lock(mmutex_feature);
        mvp_mappoints[idx] = p_mappoint;
        mn_mpts_num++;
    }
    void KeyFrame::Update_connections()
    {
        map<KeyFrame *, int> KF_counter;
        vector<MapPoint *> vp_mappoints;
        {
            unique_lock<mutex> lock(mmutex_feature);
            vp_mappoints = mvp_mappoints;
        }
        for (vector<MapPoint *>::iterator vit = vp_mappoints.begin(); vit != vp_mappoints.end(); vit++)
        {
            MapPoint *p_mpt = *vit;
            if (!p_mpt)
            {
                continue;
            }
            if (p_mpt->Is_bad())
            {
                continue;
            }
            map<KeyFrame *, int> observations = p_mpt->Get_observation();
            for (map<KeyFrame *, int>::iterator mit = observations.begin(); mit != observations.end(); mit++)
            {
                if (mit->first->mn_keyframe_id == this->mn_keyframe_id || mit->first->mb_bad || mit->first->mp_map != this->mp_map)
                {
                    continue;
                }
                KF_counter[mit->first]++;
            }
        }
        if (KF_counter.empty())
        {
            return;
        }
        int nmax = 0;
        KeyFrame *pKFmax = NULL;
        // TODO
        int th = 15;
        vector<pair<int, KeyFrame *>> v_pair;
        v_pair.reserve(KF_counter.size());
        for (map<KeyFrame *, int>::iterator mit = KF_counter.begin(); mit != KF_counter.end(); mit++)
        {
            if (mit->second > nmax)
            {
                nmax = mit->second;
                pKFmax = mit->first;
            }
            if (mit->second > th)
            {
                v_pair.push_back(make_pair(mit->second, mit->first));
                (mit->first)->Add_connection(this, mit->second);
            }
        }
        if (v_pair.empty())
        {
            v_pair.push_back(make_pair(nmax, pKFmax));
            pKFmax->Add_connection(this, nmax);
        }
        sort(v_pair.begin(), v_pair.end());
        list<KeyFrame *> l_KF;
        list<int> l_weights;
        list<pair<KeyFrame *, int>> l_keyframe_weight;
        for (int i = 0; i < v_pair.size(); i++)
        {
            l_KF.push_front(v_pair[i].second);
            l_weights.push_front(v_pair[i].first);
            l_keyframe_weight.push_front(make_pair(v_pair[i].second, v_pair[i].first));
        }
        {
            unique_lock<mutex> lock(mmutex_connection);
            mmap_keyframe_connection_weight = KF_counter;
            mvp_ordered_keyframe = vector<KeyFrame *>(l_KF.begin(), l_KF.end());
            mv_ordered_weight = vector<int>(l_weights.begin(), l_weights.end());
            mv_ordered_keyframe_weight = vector<pair<KeyFrame *, int>>(l_keyframe_weight.begin(), l_keyframe_weight.end());

            if (mb_first_connect && mn_keyframe_id != mp_map->Get_init_KFid())
            {
                mp_parent = mvp_ordered_keyframe.front();
                mp_parent->Add_child(this);
                mb_first_connect = false;
            }
        }
    }
    set<MapPoint *> KeyFrame::Get_set_mappoints()
    {
        unique_lock<mutex> lock(mmutex_feature);
        set<MapPoint *> s;
        for (int i = 0; i < mvp_mappoints.size(); i++)
        {
            if (!mvp_mappoints[i])
            {
                continue;
            }
            MapPoint *p_mp = mvp_mappoints[i];
            if (!p_mp->Is_bad())
            {
                s.insert(p_mp);
            }
        }
        return s;
    }
    float KeyFrame::Compute_scene_median_depth(int q)
    {
        vector<MapPoint *> vp_mappoints;
        Mat Tcw;

        {
            unique_lock<mutex> lock(mmutex_feature);
            unique_lock<mutex> lock2(mmutex_pose);
            vp_mappoints = mvp_mappoints;
            Tcw = mm_Tcw.clone();
        }

        vector<float> v_depth;
        v_depth.reserve(mn_feature_num);
        Mat Rcw2 = Tcw.row(2).colRange(0, 3);
        Rcw2 = Rcw2.t();
        float zcw = Tcw.at<float>(2, 3);
        for (int i = 0; i < mn_feature_num; i++)
        {
            if (vp_mappoints[i])
            {
                MapPoint *p_mpt = vp_mappoints[i];
                Mat x3dw = p_mpt->Get_world_pose();
                float z = Rcw2.dot(x3dw) + zcw;
                v_depth.push_back(z);
            }
        }
        sort(v_depth.begin(), v_depth.end());
        return v_depth[(v_depth.size() - 1) / q];
    }

    int KeyFrame::Tracked_mappoints(const int &minobs)
    {
        unique_lock<mutex> lock(mmutex_feature);
        int n_points = 0;
        const bool b_check_obs = minobs > 0;
        for (int i = 0; i < mn_feature_num; i++)
        {
            MapPoint *p_mpt = mvp_mappoints[i];
            if (p_mpt)
            {
                if (!p_mpt->Is_bad())
                {
                    if (b_check_obs)
                    {
                        if (p_mpt->Get_observation_num() >= minobs)
                        {
                            n_points++;
                        }
                    }
                    else
                    {
                        n_points++;
                    }
                }
            }
        }
        return n_points;
    }

    void KeyFrame::Set_pose(const Mat &Tcw)
    {
        unique_lock<mutex> lock(mmutex_pose);
        mm_Tcw = Tcw.clone();
        mm_Rcw = mm_Tcw.rowRange(0, 3).colRange(0, 3);
        mm_tcw = mm_Tcw.rowRange(0, 3).col(3);
        mm_Rwc = mm_Rcw.t();
        mm_twc = -mm_Rwc * mm_tcw;
        mm_Twc = Mat::eye(4, 4, CV_32F);
        mm_Rwc.copyTo(mm_Twc.rowRange(0, 3).colRange(0, 3));
        mm_twc.copyTo(mm_Twc.rowRange(0, 3).col(3));

        mm_Tcw_ = Matx44f(mm_Tcw.at<float>(0, 0), mm_Tcw.at<float>(0, 1), mm_Tcw.at<float>(0, 2), mm_Tcw.at<float>(0, 3), mm_Tcw.at<float>(1, 0), mm_Tcw.at<float>(1, 1), mm_Tcw.at<float>(1, 2), mm_Tcw.at<float>(1, 3), mm_Tcw.at<float>(2, 0), mm_Tcw.at<float>(2, 1), mm_Tcw.at<float>(2, 2), mm_Tcw.at<float>(2, 3), mm_Tcw.at<float>(3, 0), mm_Tcw.at<float>(3, 1), mm_Tcw.at<float>(3, 2), mm_Tcw.at<float>(3, 3));
        mm_Rcw_ = Matx33f(mm_Rcw.at<float>(0, 0), mm_Rcw.at<float>(0, 1), mm_Rcw.at<float>(0, 2), mm_Rcw.at<float>(1, 0), mm_Rcw.at<float>(1, 1), mm_Rcw.at<float>(1, 2), mm_Rcw.at<float>(2, 0), mm_Rcw.at<float>(2, 1), mm_Rcw.at<float>(2, 2));
        mm_tcw_ = Matx31f(mm_tcw.at<float>(0), mm_tcw.at<float>(1), mm_tcw.at<float>(2));
        mm_Twc_ = Matx44f(mm_Twc.at<float>(0, 0), mm_Twc.at<float>(0, 1), mm_Twc.at<float>(0, 2), mm_Twc.at<float>(0, 3), mm_Twc.at<float>(1, 0), mm_Twc.at<float>(1, 1), mm_Twc.at<float>(1, 2), mm_Twc.at<float>(1, 3), mm_Twc.at<float>(2, 0), mm_Twc.at<float>(2, 1), mm_Twc.at<float>(2, 2), mm_Twc.at<float>(2, 3), mm_Twc.at<float>(3, 0), mm_Twc.at<float>(3, 1), mm_Twc.at<float>(3, 2), mm_Twc.at<float>(3, 3));
        mm_Rwc_ = Matx33f(mm_Rwc.at<float>(0, 0), mm_Rwc.at<float>(0, 1), mm_Rwc.at<float>(0, 2), mm_Rwc.at<float>(1, 0), mm_Rwc.at<float>(1, 1), mm_Rwc.at<float>(1, 2), mm_Rwc.at<float>(2, 0), mm_Rwc.at<float>(2, 1), mm_Rwc.at<float>(2, 2));
        mm_twc_ = Matx31f(mm_twc.at<float>(0), mm_twc.at<float>(1), mm_twc.at<float>(2));
    }

    bool Frame::Is_inframe_field(MapPoint *p_mpt, float cos_value_th)
    {
        Mat mpt_world_pose = p_mpt->Get_world_pose();
        Mat mpt_frame_pose = mm_Rcw * mpt_world_pose + mm_tcw;

        if (mpt_frame_pose.at<float>(2) < 0.0f)
        {
            return false;
        }

        const Point2f uv = Converter::P3d_to_pointt2f(mpt_frame_pose);

        if (uv.x < mf_box_minx || uv.x > mf_box_maxx)
        {
            return false;
        }
        if (uv.y < mf_box_miny || uv.y > mf_box_maxy)
        {
            return false;
        }

        Mat OP = mpt_world_pose - mm_twc;

        const float dist = norm(OP);

        if (dist < 0.8 * p_mpt->mf_min_distance || dist > 1.2 * p_mpt->mf_max_distance)
        {
            return false;
        }

        const float view_cos = OP.dot(p_mpt->mm_normal_vector) / dist;

        if (view_cos < cos_value_th)
        {
            return false;
        }

        float ratio = p_mpt->mf_max_distance / dist;

        int nlevel = ceil(log(ratio) / log(md_scale_factor));

        if (nlevel < 0)
        {
            nlevel = 0;
        }
        else if (nlevel >= mi_nlevel)
        {
            nlevel = mi_nlevel - 1;
        }
        else
        {
            nlevel = nlevel;
        }

        p_mpt->mb_ifproj_tocurframe = true;
        p_mpt->m_preproj_uv_tocurframe = uv;
        // TODO
        //  p_mpt->mf_depth_tocurframe = mpt_frame_pose.at<float>(2);
        p_mpt->mf_viewcos_tocurframe = view_cos;
        p_mpt->mn_tarcked_level = nlevel;

        return true;
    }

    void Frame::Set_preintegration(PreIntegrationData preintegrationdata_lastframe,
                                   PreIntegrationData preintegrationdata_lastkeyframe)
    {
        m_preintegrationdata_lastframe = preintegrationdata_lastframe;
        m_preintegrationdata_lastkeyframe = preintegrationdata_lastkeyframe;
    }

    Matx31f KeyFrame::Get_tcw_()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_tcw_;
    }

    Matx31f KeyFrame::Get_twc_()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_twc_;
    }

    Matx33f KeyFrame::Get_Rwc_()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Rwc_;
    }

    Matx33f KeyFrame::Get_Rcw_()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Rcw_;
    }

    Matx44f KeyFrame::Get_Tcw_()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Tcw_;
    }

    Matx44f KeyFrame::Get_Twc_()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Twc_;
    }

    Mat KeyFrame::Get_tcw()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_tcw.clone();
    }

    Mat KeyFrame::Get_twc()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_twc.clone();
    }

    Mat KeyFrame::Get_Rwc()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Rwc.clone();
    }

    Mat KeyFrame::Get_Rcw()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Rcw.clone();
    }

    Mat KeyFrame::Get_Tcw()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Tcw.clone();
    }

    Mat KeyFrame::Get_Twc()
    {
        unique_lock<mutex> lock(mmutex_pose);
        return mm_Twc.clone();
    }

    vector<MapPoint *> KeyFrame::Get_vect_mappoints()
    {
        unique_lock<mutex> lock(mmutex_feature);
        return mvp_mappoints;
    }
    void KeyFrame::Set_map(Map *p_map)
    {
        unique_lock<mutex> lock(mmutex_map);
        mp_map = p_map;
    }
    void KeyFrame::Add_connection(KeyFrame *pKF, int weight)
    {
        {
            unique_lock<mutex> lock(mmutex_connection);
            if (!mmap_keyframe_connection_weight.count(pKF))
            {
                mmap_keyframe_connection_weight[pKF] = weight;
            }
            else if (mmap_keyframe_connection_weight[pKF] != weight)
            {
                mmap_keyframe_connection_weight[pKF] = weight;
            }
            else
            {
                return;
            }
        }
        Update_best_covisibles();
    }
    void KeyFrame::Add_child(KeyFrame *pKF)
    {
        unique_lock<mutex> lock(mmutex_connection);
        msp_children.insert(pKF);
    }
    void KeyFrame::Update_best_covisibles()
    {
        unique_lock<mutex> lock(mmutex_connection);
        vector<pair<int, KeyFrame *>> v_pair;
        v_pair.reserve(mmap_keyframe_connection_weight.size());
        for (map<KeyFrame *, int>::iterator mit = mmap_keyframe_connection_weight.begin(); mit != mmap_keyframe_connection_weight.end(); mit++)
        {
            v_pair.push_back(make_pair(mit->second, mit->first));
        }
        sort(v_pair.begin(), v_pair.end());
        list<KeyFrame *> l_KF;
        list<int> l_weight;
        list<pair<KeyFrame *, int>> l_keyframe_weight;
        for (int i = 0; i < v_pair.size(); i++)
        {
            if (!v_pair[i].second->mb_bad)
            {
                l_KF.push_front(v_pair[i].second);
                l_weight.push_front(v_pair[i].first);
                l_keyframe_weight.push_front(make_pair(v_pair[i].second, v_pair[i].first));
            }
        }
        mvp_ordered_keyframe = vector<KeyFrame *>(l_KF.begin(), l_KF.end());
        mv_ordered_weight = vector<int>(l_weight.begin(), l_weight.end());
        mv_ordered_keyframe_weight = vector<pair<KeyFrame *, int>>(l_keyframe_weight.begin(), l_keyframe_weight.end());
    }

    void KeyFrame::Update_best_covisibles(KeyFrame *p_keyframe_culled)
    {
        for (auto it = mv_ordered_keyframe_weight.begin(); it != mv_ordered_keyframe_weight.end(); it++)
        {
            if (it->first == p_keyframe_culled)
            {
                it = mv_ordered_keyframe_weight.erase(it);
            }
        }
    }

    vector<KeyFrame *> KeyFrame::Get_best_covisibility_keyframe(const int &n)
    {
        unique_lock<mutex> lock(mmutex_connection);
        if ((int)mvp_ordered_keyframe.size() < n)
        {
            return mvp_ordered_keyframe;
        }
        else
        {
            return vector<KeyFrame *>(mvp_ordered_keyframe.begin(), mvp_ordered_keyframe.begin() + n);
        }
    }

    vector<KeyFrame *> KeyFrame::Get_all_covisibility_keyframe()
    {
        unique_lock<mutex> lock(mmutex_connection);
        return mvp_ordered_keyframe;
    }

    vector<KeyFrame *> KeyFrame::Get_covisible_by_weight(const int &w)
    {
        unique_lock<mutex> lock(mmutex_connection);
        if (mvp_ordered_keyframe.empty())
        {
            return vector<KeyFrame *>();
        }
        vector<int>::iterator it = upper_bound(mv_ordered_weight.begin(), mv_ordered_weight.end(), w, KeyFrame::Weight_comp);
        if (it == mv_ordered_weight.end() && mv_ordered_weight.back() < w)
        {
            return vector<KeyFrame *>();
        }
        else
        {
            int n = it - mv_ordered_weight.begin();
            return vector<KeyFrame *>(mvp_ordered_keyframe.begin(), mvp_ordered_keyframe.begin() + n);
        }
    }

    set<KeyFrame *> KeyFrame::Get_child()
    {
        unique_lock<mutex> lock(mmutex_connection);
        return msp_children;
    }
    KeyFrame *KeyFrame::Get_parent()
    {
        unique_lock<mutex> lock(mmutex_connection);
        return mp_parent;
    }

    int KeyFrame::Sum_mappoints_by_obsnum(const int nminobs)
    {
        unique_lock<mutex> lock(mmutex_feature);
        int npoints = 0;
        for (int i = 0; i < mn_feature_num; i++)
        {
            MapPoint *p_mpt = mvp_mappoints[i];
            if (p_mpt)
            {
                if (!p_mpt->Is_bad())
                {
                    if (p_mpt->Get_observation_num() >= nminobs)
                    {
                        npoints++;
                    }
                }
            }
        }
        return npoints;
    }

    void KeyFrame::Erase_mappoint_match(const int &idx)
    {
        unique_lock<mutex> lock(mmutex_feature);
        mvp_mappoints[idx] = static_cast<MapPoint *>(NULL);
        mn_mpts_num--;
    }

    void KeyFrame::Erase_mappoint_match(MapPoint *p_mpt)
    {
        int idx = p_mpt->Get_index_in_keyframe(this);
        unique_lock<mutex> lock(mmutex_feature);
        if (idx != -1)
        {
            mvp_mappoints[idx] = static_cast<MapPoint *>(NULL);
            mn_mpts_num--;
        }
    }

    MapPoint *KeyFrame::Get_mappoint(const int &idx)
    {
        unique_lock<mutex> lock(mmutex_feature);
        return mvp_mappoints[idx];
    }

    bool KeyFrame::Is_in_image(const float &x, const float &y)
    {

        return (x >= mf_box_minx && x < mf_box_maxx && y >= mf_box_miny && y < mf_box_maxy);
    }

    void KeyFrame::Replace_mappoint_match(const int &idx, MapPoint *p_mpt)
    {
        unique_lock<mutex> lock(mmutex_feature);
        mvp_mappoints[idx] = p_mpt;
    }

    void KeyFrame::Set_bad_flag()
    {
        {
            unique_lock<mutex> lock(mmutex_connection);
            if (mn_keyframe_id == mp_map->Get_init_KFid())
            {
                return;
            }
            else if (mb_not_erase)
            {
                mb_to_be_erase = true;
                return;
            }
        }

        for (auto connection : mmap_keyframe_connection_weight)
        {
            connection.first->Erase_connection(this);
        }

        for (auto p_mpt : mvp_mappoints)
        {
            if (p_mpt)
            {
                p_mpt->Erase_observation(this);
            }
        }

        {
            unique_lock<mutex> lock(mmutex_connection);
            unique_lock<mutex> lock1(mmutex_feature);

            mmap_keyframe_connection_weight.clear();
            mvp_ordered_keyframe.clear();
            mv_ordered_keyframe_weight.clear();
            mv_ordered_weight.clear();

            set<KeyFrame *> sp_parent_candidates;
            if (mp_parent)
            {
                sp_parent_candidates.insert(mp_parent);
            }
            while (!msp_children.empty())
            {
                bool b_continue = false;
                int max = -1;
                KeyFrame *pc;
                KeyFrame *pp;
                for (auto p_keyframe : msp_children)
                {
                    if (p_keyframe->Is_bad())
                    {
                        continue;
                    }

                    vector<KeyFrame *> vp_connected = p_keyframe->Get_all_covisibility_keyframe();
                    for (auto p_keyframei : vp_connected)
                    {
                        for (auto p_candidate : sp_parent_candidates)
                        {
                            if (p_keyframei->mn_keyframe_id == p_candidate->mn_keyframe_id)
                            {
                                int w = p_keyframe->Get_weight(p_keyframei);
                                if (w > max)
                                {
                                    pc = p_keyframe;
                                    pp = p_keyframei;
                                    max = w;
                                    b_continue = true;
                                }
                            }
                        }
                    }
                }
                if (b_continue)
                {
                    pc->Change_parent(pp);
                    sp_parent_candidates.insert(pc);
                    msp_children.erase(pc);
                }
                else
                {
                    break;
                }
            }
            if (!msp_children.empty())
            {
                for (auto p_keyframe : msp_children)
                {
                    p_keyframe->Change_parent(mp_parent);
                }
            }
            if (mp_parent)
            {
                mp_parent->Erase_child(this);
                mm_T_cur_parent = mm_Tcw * mp_parent->Get_Twc();
            }

            mb_bad = true;
        }

        mp_map->Erase_keyframe(this);
        // TODO keyframedb
    }

    void KeyFrame::Erase_connection(KeyFrame *p_keyframe)
    {
        bool b_update = false;
        {
            unique_lock<mutex> lock(mmutex_connection);
            if (mmap_keyframe_connection_weight.count(p_keyframe))
            {
                mmap_keyframe_connection_weight.erase(p_keyframe);
                b_update = true;
            }
        }
        if (b_update)
        {
            Update_best_covisibles();
        }
    }

    int KeyFrame::Get_weight(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_connection);
        if (mmap_keyframe_connection_weight.count(p_keyframe))
        {
            return mmap_keyframe_connection_weight[p_keyframe];
        }
        else
        {
            return 0;
        }
    }

    void KeyFrame::Change_parent(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_connection);
        if (p_keyframe == this)
        {
            cout << "error change parent keyframe, the parent and children are the same." << endl;
        }
        mp_parent = p_keyframe;
        p_keyframe->Add_child(this);
    }

    void KeyFrame::Erase_child(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_connection);
        msp_children.erase(p_keyframe);
    }

    void KeyFrame::Set_as_ref_keyframe(bool flag)
    {
        unique_lock<mutex> lock(mmutex_member);
        mb_used_as_ref_keyframe = flag;
    }

    bool KeyFrame::Used_as_ref_keyframe()
    {
        unique_lock<mutex> lock(mmutex_member);
        return mb_used_as_ref_keyframe;
    }

    void KeyFrame::Set_not_erase()
    {
        unique_lock<mutex> lock(mmutex_connection);
        mb_not_erase = true;
    }

    void KeyFrame::Set_erase()
    {
        {
            unique_lock<mutex> lock(mmutex_connection);
            if (msp_loop_edge.empty())
            {
                mb_not_erase = false;
            }
        }
        if (mb_to_be_erase)
        {
            Set_bad_flag();
        }
    }

    void KeyFrame::Add_loopedge(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_connection);
        mb_not_erase = true;
        msp_loop_edge.insert(p_keyframe);
    }

    bool KeyFrame::Has_children(KeyFrame *p_keyframe)
    {
        unique_lock<mutex> lock(mmutex_connection);
        return msp_children.count(p_keyframe);
    }

    set<KeyFrame *> KeyFrame::Get_loop_edge()
    {
        unique_lock<mutex> lock(mmutex_connection);
        return msp_loop_edge;
    }

    set<KeyFrame *> KeyFrame::Get_connect_keyframe()
    {
        unique_lock<mutex> lock(mmutex_connection);
        set<KeyFrame *> s;
        for (auto keyframe_weight : mmap_keyframe_connection_weight)
        {
            s.insert(keyframe_weight.first);
        }
        return s;
    }

}
